#include"CameraSetting.hpp"




namespace tools{
   inline float TimeStep = 1.0f; // 定义操作都是在tools中
}


// 摄像头参数
void CameraSetting::setupCamera(void *handle){
    int nRet = MV_OK;

    // 关闭自动曝光模式
    nRet = MV_CC_SetEnumValue(handle, "ExposureAuto", 0); // 0表示手动模式
    // nRet = MV_CC_SetEnumValue(handle, "GammaAuto", 0); // 0=手动模式，1=自动模式
    nRet = MV_CC_SetFloatValue(handle, "GainAuto", 0);
    // 参数调节 
    float targetExposure = 2500;  // 目标曝光时间 
    // float targetGamma = 1.5f; // 设置伽马值为1.5（增强暗部细节）
    float targetGain = 9.0f; // 你需要的增益值（单位通常为dB，具体范围查相机手册）

    nRet = MV_CC_SetFloatValue(handle, "ExposureTime", targetExposure);
    if (nRet != MV_OK) {
        printf("Set ExposureTime failed! nRet = 0x%x\n", nRet);
    }

    nRet = MV_CC_SetFloatValue(handle, "Gain", targetGain);
    if (nRet != MV_OK) {
        printf("Set Gain failed! nRet = 0x%x\n", nRet);
    }

    // nRet = MV_CC_SetFloatValue(handle, "Gamma", targetGamma);
    // if (nRet != MV_OK) {
    //     printf("Set Gamma failed! nRet = 0x%x\n", nRet);
    // }
    MV_CC_SetIntValue(handle, "Width", 1440);   // 设置采集宽度
    MV_CC_SetIntValue(handle, "Height", 1080);  // 设置采集高度
}


// 海康相机API
void CameraSetting::import_image(){
    int nRet = MV_OK;
    void* handle = NULL;
    // 1. 枚举设备
    MV_CC_DEVICE_INFO_LIST stDeviceList;
    memset(&stDeviceList, 0, sizeof(MV_CC_DEVICE_INFO_LIST));
    nRet = MV_CC_EnumDevices(MV_GIGE_DEVICE | MV_USB_DEVICE, &stDeviceList);
    if (nRet != MV_OK || stDeviceList.nDeviceNum == 0) {
        printf("No camera found or enum failed! nRet = 0x%x\n", nRet);
        return;
    }
    // 2. 选择设备并创建句柄
    unsigned int nIndex = 0;
    // printf("Please input camera index (0~%d): ", stDeviceList.nDeviceNum - 1);
    // scanf("%d", &nIndex);
    if (nIndex >= stDeviceList.nDeviceNum) {
        printf("Input error!\n");
        return;
    }
    nRet = MV_CC_CreateHandle(&handle, stDeviceList.pDeviceInfo[nIndex]);
    if (nRet != MV_OK) {
        printf("Create handle failed! nRet = 0x%x\n", nRet);
        return;
    }
    // 3. 打开设备
    nRet = MV_CC_OpenDevice(handle);
    if (nRet != MV_OK) {
        printf("Open device failed! nRet = 0x%x\n", nRet);
        MV_CC_DestroyHandle(handle);
        return;
    }
    // 设置相机参数
    setupCamera(handle);

    // // 在相机初始化后添加
    // MV_CC_SetBoolValue(handle, "AcquisitionFrameRateEnable", true);
    // float maxFPS = 0;
    // MV_CC_GetFloatValue(handle, "AcquisitionFrameRate", &maxFPS);
    // std::cout << "相机最大帧率: " << maxFPS << "FPS" << std::endl;

    // 4. 设置触发模式为off（连续采集）
    nRet = MV_CC_SetEnumValue(handle, "TriggerMode", 0);
    if (nRet != MV_OK) {
        printf("Set TriggerMode failed! nRet = 0x%x\n", nRet);
    }
    // 5. 开始取流
    nRet = MV_CC_StartGrabbing(handle);
    if (nRet != MV_OK) {
        printf("Start grabbing failed! nRet = 0x%x\n", nRet);
        MV_CC_CloseDevice(handle);
        MV_CC_DestroyHandle(handle);
        return;
    }
    // 6. 获取一帧图像
    MV_FRAME_OUT_INFO_EX stImageInfo = {0};
    unsigned char* pData = (unsigned char*)malloc(1440*1080*3); // 预分配空间 1920*1080*3  1280*1080*3
    unsigned char* pDstBuf = nullptr; 
    
    // // ==== 新增：定义VideoWriter ====
    // cv::VideoWriter writer;
    // bool isWriterOpened = false;

    
    

    // 打开串口
    if (serial.open(config)) {
        std::cout << "串口打开成功" << std::endl;
    } else {
        std::cout << "串口打开失败: " << serial.getLastError() <<"****"<< std::endl;
    }
    
    while (true) {
        // 记录当前时间用于FPS计算
        auto currentTime = std::chrono::high_resolution_clock::now();
        frameTimes.push_back(currentTime);// 获取当前时间戳
        
        // 保持缓冲区大小固定 维护长度，太长则挤出去一个
        if (frameTimes.size() > frameTimeBufferSize) {
            frameTimes.pop_front();
        }

        nRet = MV_CC_GetOneFrameTimeout(handle, pData, 1440*1080*3, &stImageInfo, 30); // 约30FPS的超时时间
        if (nRet == MV_OK) { // 这里很容易崩 也就是为false,很不稳定的原因是什么？
            cv::Mat img;
            static bool isInit = false;
            if (stImageInfo.enPixelType == PixelType_Gvsp_Mono8) {
                img = cv::Mat(stImageInfo.nHeight, stImageInfo.nWidth, CV_8UC1, pData);
            } else if (stImageInfo.enPixelType == PixelType_Gvsp_RGB8_Packed) {
                img = cv::Mat(stImageInfo.nHeight, stImageInfo.nWidth, CV_8UC3, pData);
                cv::cvtColor(img, img, cv::COLOR_RGB2BGR);
            } else if (stImageInfo.enPixelType == PixelType_Gvsp_BayerRG8) {
                // 只在第一次需要时分配内存，避免重复分配释放
                if (!isInit) {
                    pDstBuf = (unsigned char*)malloc(stImageInfo.nWidth * stImageInfo.nHeight * 3);
                    isInit = true;
                }
                
                MV_CC_PIXEL_CONVERT_PARAM stConvertParam = {0};
                stConvertParam.nWidth = stImageInfo.nWidth;
                stConvertParam.nHeight = stImageInfo.nHeight;
                stConvertParam.pSrcData = pData;
                stConvertParam.nSrcDataLen = stImageInfo.nFrameLen;
                stConvertParam.enSrcPixelType = stImageInfo.enPixelType;
                // 核心优化：直接让SDK输出BGR格式，省去手动交换步骤
                stConvertParam.enDstPixelType = PixelType_Gvsp_BGR8_Packed;  // 目标格式改为BGR
                stConvertParam.pDstBuffer = pDstBuf;
                stConvertParam.nDstBufferSize = stImageInfo.nWidth * stImageInfo.nHeight * 3;
                int nRetConvert = MV_CC_ConvertPixelType(handle, &stConvertParam);
                if (nRetConvert == MV_OK) {
                    // 直接使用BGR格式图像，无需再调用cvtColor
                    cv::Mat img(stImageInfo.nHeight, stImageInfo.nWidth, CV_8UC3, pDstBuf);
                    // // 记录开始处理图象所消耗的时间
                    // auto start = chrono::high_resolution_clock::now();
                    resize(img, img, cv::Size(1440, 1080)); // 在这里进行尺寸的缩放有利于图像处理的速度得到增强(个人理解)
                    Mat show = process::get_armor(img);
                    // line(img,Point(720,520),Point(720,560),Scalar(120,220,20),2);
                    // line(img,Point(700,540),Point(740,540),Scalar(120,220,20),2);
                    cv::circle(show,Point(720,540),5,Scalar(50,250,250),-1); // firePoint 
                    cv::imshow("Camera Image", show);
                    

                    // // 记录处理图象所消耗的结束时间
                    // auto end = chrono::high_resolution_clock::now();
                    // auto duration = chrono::duration_cast<chrono::milliseconds>(end - start);
                    // // cout  << duration.count() << " ms" << endl;
                    
                // // ==== 新增：初始化VideoWriter ====
                // if (!isWriterOpened && !show.empty()) {
                //     int fourcc = cv::VideoWriter::fourcc('X','V','I','D'); // 或 'M','J','P','G'
                //     double fps = 30.0; // 你期望的帧率
                //     cv::Size size(img.cols, img.rows);
                //     writer.open("/home/nano/Rm_vison/c511_vision/SaveGraph/output.avi", fourcc, fps, size, img.channels() == 3);
                //     if (!writer.isOpened()) {
                //         printf("VideoWriter open failed!\n");
                //     } else {
                //         isWriterOpened = true;
                //     }
                // }
                // // ==== 新增：写入帧 ====
                // if (isWriterOpened && !show.empty()) {
                //     writer.write(img);
                // }
                // ///--------------------------------------
                } else {
                    printf("像素格式转换失败! nRet = 0x%x\n", nRetConvert);
                }
            } else {
                printf("Unsupported pixel format! PixelType: 0x%lx\n", stImageInfo.enPixelType);
            }
            
        } else {
            printf("Get frame failed! nRet = 0x%x\n", nRet);
            break;
        }        
        if (cv::waitKey(1) == 27) break; // 按ESC退出
    }
    // 0x80000007 像素格式错误
    free(pData);
    // 在这里释放pDstBuf（循环外）
    if (pDstBuf) {
        free(pDstBuf);
        pDstBuf = nullptr;
    }
    // // ==== 新增：释放VideoWriter ====
    // if (isWriterOpened) {
    //     writer.release();
    // }
    
    // 8. 停止取流，关闭设备，销毁句柄
    MV_CC_StopGrabbing(handle);
    MV_CC_CloseDevice(handle);
    MV_CC_DestroyHandle(handle);
}
// 计算帧率
// 用这个代码来计算帧率的卡尔曼效果会好很多
float CameraSetting::calculateFPS() {
    if (frameTimes.size() < 2) return 0.0f;
    
    // 计算第一个和最后一个的时间戳相差的时间
    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(
        frameTimes.back() - frameTimes.front()
    ).count();// ".count()是为了提取duration对象中的数值"

    // cout<<duration<<"ms"<<endl;//整个从摄像头获取到结束的延迟
    tools::TimeStep = duration/1000.0f;//单位转换ms转m
    
    if (duration <= 0) return 0.0f;
    
    return (frameTimes.size() - 1) * 1000.0f / duration; 
}

// 2、
//
// （事实上这种方法会更好）有待研究
// float CameraSetting::calculateFPS() {
//     if (frameTimes.size() < 2) return 0.0f;
//     // 使用最新两帧计算瞬时时间差（用于TimeStep）
//     auto instant_duration = std::chrono::duration_cast<std::chrono::microseconds>(
//         frameTimes.back() - *(frameTimes.end()-2));
//     TimeStep = instant_duration.count() / 1000000.0f; // 秒单位
//     // 低通滤波抑制噪声
//     static float smoothed_dt = 0.02f; // 初始值20ms
//     smoothed_dt = 0.8f * smoothed_dt + 0.2f * TimeStep;
//     TimeStep = smoothed_dt;
//     // 计算瞬时FPS
//     float instantFPS = 1000000.0f / instant_duration.count(); // 使用微秒确保精度
//     // 调试输出
//     std::cout << "TimeStep = " << TimeStep << "s (" << TimeStep*1000 << "ms) | "
//               << "InstantFPS = " << instantFPS << "FPS" << std::endl;
//     return instantFPS;
// }
//
// 3、
// float CameraSetting::calculateFPS() {
//     if (frameTimes.size() < 2) return 0.0f;
//     // 平均帧间隔计算（用于TimeStep）
//     auto avg_duration = std::chrono::duration_cast<std::chrono::milliseconds>(
//         frameTimes.back() - frameTimes.front());
//     TimeStep = avg_duration.count() / (1000.0f * (frameTimes.size() - 1));
//     // 瞬时帧间隔计算（用于FPS显示）
//     auto instant_duration = std::chrono::duration_cast<std::chrono::microseconds>(
//         frameTimes.back() - *(frameTimes.end()-2));
//     float instantFPS = 1000000.0f / instant_duration.count();  // 使用微秒确保精度
//     // // 调试输出
//     // std::cout << "TimeStep = " << TimeStep 
//     //           << " | duration = " << avg_duration.count()
//     //           << " | InstantFPS = " << instantFPS << std::endl;
//     // return instantFPS;
//     // 数学原理：FPS = 帧数/总时间 × 1000(ms/s) [ 平滑稳定的帧率显示 ] 
//     // return (frameTimes.size() - 1) * 1000.0f / avg_duration.count(); // 
//     return 1.0/TimeStep;
// }

// 在图像上显示帧率
void CameraSetting::displayFPS(cv::Mat& image, float fps) {
    std::stringstream ss;
    ss << "FPS: " << std::fixed << std::setprecision(1) << fps;
        
    cv::putText(
        image, 
        ss.str(), 
        cv::Point(10, 30),  // 左上角位置
        cv::FONT_HERSHEY_SIMPLEX, 
        0.7,                // 字体大小
        cv::Scalar(0, 255, 0),  // 绿色
        2,                  // 线宽
        cv::LINE_AA         // 抗锯齿
    );
}




